package grid;

import hdgeom.interfaces.HasInside;
import hdgeom.primitives.Box;
import hdgeom.primitives.Line;
import hdgeom.primitives.TrianglePreCalc;

import java.io.BufferedInputStream;
import java.io.BufferedOutputStream;
import java.io.BufferedWriter;
import java.io.DataInputStream;
import java.io.DataOutputStream;
import java.io.File;
import java.io.FileInputStream;
import java.io.FileNotFoundException;
import java.io.FileOutputStream;
import java.io.FileWriter;
import java.io.IOException;
import java.util.Arrays;

import mmesh.MMesh;
import processing.core.PGraphics;
import processing.core.PVector;
import simpleMesh.Face;
import simpleMesh.Mesh;
import solid.Node;
//import solid.Node;
import solid.SimpleMCube;

public class Grid3DFloat extends AbstractGrid {

	static float SMALL_NUM = 0.00000001f;
	// 0 = disjoint (no intersect)
	// 1 = intersect in unique point I1
	// 2 = are in the same plane
	public float[] values;
	public float d1 = 1f;
	/**
 * 
 */
	public float d2 = (float) Math.sqrt(2);
	/**
 * 
 */
	public float d3 = (float) Math.sqrt(3);
	/**
 * 
 */

	SimpleMCube mCube;
	/**
 * 
 */

	public final static int WHITE = 0;
	/**
 * 
 */
	public final static int BLACK = 1;
	public Grid3DFloat() {
		
	}
	public Grid3DFloat(int nX, int nY, int nZ) {
		values = new float[nX * nY * nZ];
		this.nX = nX;
		this.nY = nY;
		this.nZ = nZ;
		update();
	}

	public Grid3DFloat(float[] values, int nX, int nY, int nZ) {
		this.values = values;
		this.nX = nX;
		this.nY = nY;
		this.nZ = nZ;
		update();
	}

	public Grid3DFloat(String fileName) {
		values = loadValues(fileName);
	}

	public float get(int x, int y, int z) {
		return values[getIndex(x, y, z)];
	}

	public float get(int index) {
		return values[index];
	}

	public void set(int x, int y, int z, float value) {
		values[getIndex(x, y, z)] = value;
	}

	public void set(int index, float value) {
		values[index] = value;
	}

	public void setValueToAll(float value) {
		Arrays.fill(values, value);
	}

	public void setValue(int x1, int y1, int z1, int x2, int y2, int z2,
			float value) {
		for (int x = x1; x < x2; x++) {
			for (int y = y1; y < y2; y++) {
				for (int z = z1; z < z2; z++) {
					this.set(x, y, z, value);
				}
			}
		}
	}

	public void setValue(int cx, int cy, int cz, float r, float value) {
		float rq = r * r;
		for (int x = 0; x < nX; x++) {
			for (int y = 0; y < nY; y++) {
				for (int z = 0; z < nZ; z++) {
					int dX = cx - x;
					int dY = cy - y;
					int dZ = cz - z;
					if ((dX * dX + dY * dY + dZ * dZ) < rq) {
						this.set(x, y, z, value);
					}
				}
			}
		}
	}

	/**
	 * adds the value at the position x,y,z
	 * 
	 * @param x
	 * @param y
	 * @param z
	 * @param value
	 */
	public void add(int x, int y, int z, float value) {
		values[getIndex(x, y, z)] += value;
	}

	/**
	 * subtracts the value at the position x,y,z
	 * 
	 * @param x
	 * @param y
	 * @param z
	 * @param value
	 */
	public void sub(int x, int y, int z, float value) {
		values[getIndex(x, y, z)] -= value;
	}

	/**
	 * sets 1 to cell where mask is 1
	 * 
	 * @param mask
	 */
	public void add(Grid3DFloat mask) {
		for (int i = 0; i < values.length; i++) {
			if (mask.get(i) == 1) {
				values[i] = 1;
			}
		}
	}

	/**
	 * sets 0 to cell where mask is 1
	 * 
	 * @param mask
	 */
	public void subtract(Grid3DFloat mask) {
		for (int i = 0; i < values.length; i++) {
			if (mask.get(i) == 1) {
				values[i] = 0;
			}
		}
	}

	/**
	 * sets 1 to cell where mask and cell is 1, otherwise 0
	 * 
	 * @param mask
	 */
	public void intersect(Grid3DFloat mask) {
		for (int i = 0; i < values.length; i++) {
			if (mask.get(i) == 1 && values[i] == 1) {
				values[i] = 1;
			} else {
				values[i] = 0;
			}
		}
	}

	/**
	 * sets 0 to cell where mask and cell is 1, otherwise 1 if one of them is
	 * one
	 * 
	 * @param mask
	 */
	public void exclusiveOr(Grid3DFloat mask) {
		for (int i = 0; i < values.length; i++) {
			if (mask.get(i) == 1 && values[i] == 1) {
				values[i] = 0;
			} else if (values[i] == 1 || mask.get(i) == 1) {
				values[i] = 1;
			}
		}
	}

	/**
	 * @param v
	 * @param min
	 * @param max
	 * @return
	 */
	public int constrain(int v, int min, int max) {
		v = Math.min(v, max);
		v = Math.max(v, min);
		return v;
	}

	/**
	 * @return
	 */
	public float getMin() {
		float minV = Float.POSITIVE_INFINITY;
		for (int i = 0; i < values.length; i++) {
			minV = Math.min(values[i], minV);
		}
		return minV;
	}

	/**
	 * @return
	 */
	public float getMax() {
		float maxV = Float.NEGATIVE_INFINITY;
		for (int i = 0; i < values.length; i++) {
			maxV = Math.max(values[i], maxV);
		}
		return maxV;
	}

	public void normalize() {
		normalize(values);
	}
	//based on integer Coordinates, without dim and shift
	public float getMappedValueZ(int x,int y, float sourceZ){
		sourceZ=(sourceZ-z1)/dim;
		int cZ0 = (int)sourceZ ;
		int cZ1 = cZ0 + 1;
		float v0 = get(x, y, cZ0);
		float v1 = get(x, y, cZ1);
		float v = map(sourceZ, cZ1, cZ0, v1, v0);
		return v;
	}
	
public float getMappedValue(float sourceX,float sourceY,float sourceZ){
	sourceX=(sourceX-x1)/dim;
	sourceY=(sourceY-y1)/dim;
	sourceZ=(sourceZ-z1)/dim;
	int cX0 = (int) (sourceX);
	int cY0 = (int) (sourceY);
	int cZ0 = (int) (sourceZ);
	int cX1 = cX0 + 1;
	int cY1 = cY0 + 1;
	int cZ1 = cZ0 + 1;

	float v000 = get(cX0, cY0, cZ0);
	float v100 = get(cX1, cY0, cZ0);
	float v010 = get(cX0, cY1, cZ0);
	float v110 = get(cX1, cY1, cZ0);
	
	float v00 = map(sourceX, cX1, cX0, v000, v100);
	float v10 = map(sourceX, cX1, cX0, v010, v110);
	
	if (v00>1)System.out.println(v000+" "+v100);
	
	float v0 = map(sourceY, cY1, cY0, v00, v10);
	
	
	float v001 = get(cX0, cY0, cZ1);
	float v101 = get(cX1, cY0, cZ1);
	float v011 = get(cX0, cY1, cZ1);
	float v111 = get(cX1, cY1, cZ1);
	float v01 = map(sourceX, cX1, cX0, v001, v101);
	float v11 = map(sourceX, cX1, cX0, v011, v111);
	float v1 = map(sourceY, cY1, cY0, v01, v11);
	
	
	float v = map(sourceZ, cZ1, cZ0, v0, v1);
	return v;
}
	public static void mapValues(float[] values, float min, float max) {
		float minV = Float.POSITIVE_INFINITY;
		float maxV = Float.NEGATIVE_INFINITY;
		for (int i = 0; i < values.length; i++) {
			minV = Math.min(values[i], minV);
			maxV = Math.max(values[i], maxV);
		}
		for (int i = 0; i < values.length; i++) {
			values[i] = map(values[i], minV, maxV, min, max);
		}
	}

	public static void normalize(float[] values) {
		mapValues(values, 0, 1);

	}

	/**
	 * @param startIndex
	 * @param iso
	 */
	public void reached(int startIndex, float iso) {
		Grid3DReached.reached(values, nX, nY, nZ, startIndex, iso);
	}

	/**
	 * @param mask
	 * @param values
	 * @param nX
	 * @param nY
	 * @param nZ
	 */
	public static void selectBorders(Grid3DFloat mask, float[] values, int nX,
			int nY, int nZ) {
		int nYZ = nY * nZ;
		int nbIndex;
		int index;
		for (int x = 0; x < nX; x++) {
			for (int y = 0; y < nY; y++) {
				for (int z = 0; z < nZ; z++) {
					index = (x) * nYZ + y * nZ + z;
					if (x > 0) {
						nbIndex = (x - 1) * nYZ + y * nZ + z;
						if (values[nbIndex] != values[index]) {
							mask.values[index] = 1;
						}
					}
					if (x < nX - 1) {
						nbIndex = (x + 1) * nYZ + y * nZ + z;
						if (values[nbIndex] != values[index]) {
							mask.values[index] = 1;
						}
					}
					if (y > 0) {
						nbIndex = (x) * nYZ + (y - 1) * nZ + z;
						if (values[nbIndex] != values[index]) {
							mask.values[index] = 1;
						}
					}
					if (y < nY - 1) {
						nbIndex = (x) * nYZ + (y + 1) * nZ + z;
						if (values[nbIndex] != values[index]) {
							mask.values[index] = 1;
						}
					}
					if (z > 0) {
						nbIndex = (x) * nYZ + (y) * nZ + z - 1;
						if (values[nbIndex] != values[index]) {
							mask.values[index] = 1;
						}
					}
					if (z < nZ - 1) {
						nbIndex = (x) * nYZ + (y) * nZ + z + 1;
						if (values[nbIndex] != values[index]) {
							mask.values[index] = 1;
						}
					}
				}
			}
		}
	}

	/**
	 * @param mask
	 */
	public void selectBorders(Grid3DFloat mask) {
		selectBorders(mask, values, nX, nY, nZ);
	}

	public void saveValues(String fileName) {
		try {
			FileOutputStream fos = new FileOutputStream(fileName);
			DataOutputStream dos = new DataOutputStream(
					new BufferedOutputStream(fos));
			dos.writeInt(nX);
			dos.writeInt(nY);
			dos.writeInt(nZ);
			dos.writeFloat(x1);
			dos.writeFloat(y1);
			dos.writeFloat(z1);
			dos.writeFloat(dim);
			for (int j = 0; j < values.length; j++) {
				dos.writeFloat(values[j]);
			}
			dos.flush();
			dos.close();
			fos.flush();
			fos.close();
		} catch (FileNotFoundException ex) {
			System.out.println("FileNotFoundException : " + ex);
		} catch (IOException ioe) {
		}
	}

	public void saveValues(float[] source, String fileName) {
		try {
			FileOutputStream fos = new FileOutputStream(fileName);
			DataOutputStream dos = new DataOutputStream(
					new BufferedOutputStream(fos));
			dos.writeInt(nX);
			dos.writeInt(nY);
			dos.writeInt(nZ);
			dos.writeFloat(x1);
			dos.writeFloat(y1);
			dos.writeFloat(z1);
			dos.writeFloat(dim);
			for (int j = 0; j < source.length; j++) {
				dos.writeFloat(source[j]);
			}
			dos.flush();
			dos.close();
			fos.flush();
			fos.close();
		} catch (FileNotFoundException ex) {
			System.out.println("FileNotFoundException : " + ex);
		} catch (IOException ioe) {
		}
	}

	public float[] loadValues(String fileName) {
		FileInputStream fin;
		float[] target = null;
		try {
			fin = new FileInputStream(fileName);
			BufferedInputStream bin = new BufferedInputStream(fin);
			DataInputStream din = new DataInputStream(bin);
			try {
				nX = din.readInt();
				nY = din.readInt();
				nZ = din.readInt();
				x1 = din.readFloat();
				y1 = din.readFloat();
				z1 = din.readFloat();
				dim = din.readFloat();
				update();
			} catch (IOException e1) {
				// TODO Auto-generated catch block
				e1.printStackTrace();
			}
			target = new float[nX * nY * nZ];
			for (int i = 0; i < target.length; i++) {
				try {
					target[i] = din.readFloat();
				} catch (IOException e) {
					// TODO Auto-generated catch block
					e.printStackTrace();
				}
			}
		} catch (FileNotFoundException e1) {
			// TODO Auto-generated catch block
			e1.printStackTrace();
		}
		return target;
	}

	public void loadValues(String fileName, float[] target) {
		FileInputStream fin;

		try {
			fin = new FileInputStream(fileName);
			BufferedInputStream bin = new BufferedInputStream(fin);
			DataInputStream din = new DataInputStream(bin);
			try {
				nX = din.readInt();
				nY = din.readInt();
				nZ = din.readInt();
				x1 = din.readFloat();
				y1 = din.readFloat();
				z1 = din.readFloat();
				dim = din.readFloat();
				update();
			} catch (IOException e1) {
				// TODO Auto-generated catch block
				e1.printStackTrace();
			}

			for (int i = 0; i < target.length; i++) {
				try {
					target[i] = din.readFloat();
				} catch (IOException e) {
					// TODO Auto-generated catch block
					e.printStackTrace();
				}
			}

		} catch (FileNotFoundException e1) {
			// TODO Auto-generated catch block
			e1.printStackTrace();
		}

	}

	public boolean hasDifferentNb2D(int x, int y, int z) {
		int index = getIndex(x, y, z);
		float v = get(x, y, z);

		// check all nbs if one is bigger iso return true;
		if (x == 0)
			return false;
		if (get(x - 1, y, z) != v)
			return true;

		if (x == nX - 1)
			return false;
		if (get(x + 1, y, z) != v)
			return true;

		if (y == 0)
			return false;
		if (get(x, y - 1, z) != v)
			return true;

		if (y == nY - 1)
			return false;
		if (get(x, y + 1, z) != v)
			return true;

		return false;
	}

	public boolean isBorder(int x, int y, int z, float iso) {
		int index = getIndex(x, y, z);
		if (get(index) < iso)
			return false;
		// check all nbs if one is bigger iso return true;
		if (x == 0)
			return false;
		if (get(x - 1, y, z) < iso)
			return true;

		if (x == nX - 1)
			return false;
		if (get(x + 1, y, z) < iso)
			return true;

		if (y == 0)
			return false;
		if (get(x, y - 1, z) < iso)
			return true;

		if (y == nY - 1)
			return false;
		if (get(x, y + 1, z) < iso)
			return true;

		if (z == 0)
			return false;
		if (get(x, y, z - 1) < iso)
			return true;

		if (z == nZ - 1)
			return false;
		if (get(x, y, z + 1) < iso)
			return true;

		return false;
	}

	/**
	 * @param value
	 * @param offset
	 * @param mask
	 */
	public void select(float value, float offset, Grid3DFloat mask) {
		for (int i = 0; i < values.length; i++) {
			if (Math.abs(values[i] - value) < offset) {
				mask.values[i] = 1;
			} else {
				mask.values[i] = 0;
			}
		}
	}

	/**
	 * @param offset
	 * @param mask
	 */
	public static void offsetSelection(float offset, Grid3DFloat mask) {
		float[] depth = new float[mask.values.length];
		if (offset > 0) {
			for (int i = 0; i < mask.values.length; i++) {
				if (mask.get(i) == 1) {
					depth[i] = 0;
				} else {
					depth[i] = 1000000;
				}
			}
			Grid3DDepth.calculateDepth(depth, mask.nX, mask.nY, mask.nZ, mask.d1, mask.d2, mask.d3);
			for (int i = 0; i < depth.length; i++) {
				if (depth[i] < offset) {
					mask.values[i] = 1;
				}
			}
		} else {
			for (int i = 0; i < mask.values.length; i++) {
				if (mask.get(i) != 1) {
					depth[i] = 0;
				} else {
					depth[i] = 1000000;
				}
			}
			Grid3DDepth.calculateDepth(depth, mask.nX, mask.nY, mask.nZ, mask.d1, mask.d2, mask.d3);
			for (int i = 0; i < depth.length; i++) {
				if (depth[i] < Math.abs(offset)) {
					mask.set(i, 1);
				}
			}
		}
	}

	/**
	 * Sets the value to all cells inside a HasInside object
	 * 
	 * @param volume
	 *            A mesh-boundary
	 * @param value
	 *            The value
	 */
	public void setValue(HasInside volume, float value) {
		for (int x = 0; x < nX; x++) {
			for (int y = 0; y < nY; y++) {
				for (int z = 0; z < nZ; z++) {
					if (volume.isInside(x, y, z)) {
						this.set(x, y, z, value);
					}
				}
			}
		}
	}

	/**
	 * @param x
	 * @param y
	 * @param z
	 * @return
	 */
	public boolean isInside(int x, int y, int z) {
		if (x < 0 || x > nX - 1)
			return false;
		if (y < 0 || y > nY - 1)
			return false;
		if (z < 0 || z > nZ - 1)
			return false;
		return true;
	}

	/**
	 * inverts the values between 1 and 0
	 */
	public void invert() {
		for (int i = 0; i < values.length; i++) {
			values[i] = 1 - values[i];
		}
	}

	/**
	 * @param x1
	 * @param y1
	 * @param z1
	 * @param x2
	 * @param y2
	 * @param z2
	 * @param r1
	 */
	public void distSegment(float x1, float y1, float z1, float x2, float y2,
			float z2, float r1) {
		Box b3d = new Box();
		b3d.addPoint(x1, y1, z1);
		b3d.addPoint(x2, y2, z2);
		b3d.offset(r1 + 1);
		Line line = new Line(x1, y1, z1, x2, y2, z2);
		int ix1 = (int) Math.max(0, b3d.x1);
		int iy1 = (int) Math.max(0, b3d.y1);
		int iz1 = (int) Math.max(0, b3d.z1);
		int ix2 = (int) Math.min(nX - 1, b3d.x2);
		int iy2 = (int) Math.min(nY - 1, b3d.y2);
		int iz2 = (int) Math.min(nZ - 1, b3d.z2);
		for (int x = ix1; x < ix2; x++) {
			for (int y = iy1; y < iy2; y++) {
				for (int z = iz1; z < iz2; z++) {
					float cD = line.distToSeg(x, y, z);
					if (line.distToSeg(x, y, z) < r1) {
						if (this.get(x, y, z) < r1 - cD)
							;
						this.set(x, y, z, r1 - cD);
					}
				}
			}
		}
	}

	/**
	 * @param iso
	 */
	public void createMCube(float iso) {
		mCube = new SimpleMCube();
		mCube.marchingCubes(nX, nY, nZ, values, iso, 1, 0, 0, 0, null);
	}

	/**
	 * @param iso
	 * @param file
	 */
	public void saveMCube(float iso, String file) {
		mCube.saveObj(file);
		// mCube.marchingCubes(nX, nY, nZ, values, iso, 1, 0, 0, 0, file);
	}

	/**
	 * sets value to all cells where mask is 1
	 * 
	 * @param mask
	 * @param value
	 */
	public void fill(Grid3DFloat mask, float value) {
		for (int i = 0; i < values.length; i++) {
			if (mask.get(i) == 1) {
				values[i] = value;
			}
		}
	}

	/**
	 * copies the values from the mask
	 * 
	 * @param mask
	 */
	public void copy(Grid3DFloat mask) {
		for (int i = 0; i < values.length; i++) {
			values[i] = mask.values[i];
		}
	}

	public void fill(Mesh mesh, float value) {
		Box b3d = mesh.getBounds();
		int x1 = Math.max(0, (int) b3d.x1 - 1);
		int x2 = Math.min(nX, (int) (b3d.x2 + 2));
		int y1 = Math.max(0, (int) b3d.y1 - 1);
		int y2 = Math.min(nY, (int) (b3d.y2 + 2));
		int z1 = Math.max(0, (int) b3d.z1 - 1);
		int z2 = Math.min(nZ, (int) (b3d.z2 + 2));
		for (int x = x1; x < x2; x++) {
			for (int y = y1; y < y2; y++) {
				for (int z = z1; z < z2; z++) {
					if (mesh.isInside(x, y, z)) {
						set(x, y, z, value);
					}
				}
			}
		}
	}

	/**
	 * sets value to all cells inside a mesh
	 * 
	 * @param mesh
	 * @param value
	 */

	public MMesh getMarchinCubeMesh(float iso) {
		SimpleMCube mCube = new SimpleMCube();
		mCube.marchingCubes(nX, nY, nZ, values, iso, 1, 0, 0, 0, null);
		MMesh mMesh = new MMesh();
		mMesh.xc = mCube.xc;
		mMesh.yc = mCube.yc;
		mMesh.zc = mCube.zc;
		mMesh.triangles = mCube.triangles;
		return mMesh;
	}

	/*
	 * public int[]getQuadMesh(PVector[]nodes,float iso){ ArrayList<int[]> for
	 * (int x=0;x<nX;x++){ for (int y=0;y<nY;y++){ for (int z=0;z<nZ;z++){ float
	 * v=get(x,y,z);
	 * 
	 * if (x<nX-1){ float vX=get(x+1,y,z); if (v<iso!=vX<iso){ Face face=new
	 * Face(ng[x+1][y][z],ng[x+1][y][z+1],ng[x+1][y+1][z+1],ng[x+1][y+1][z]); if
	 * (v<iso)face.turn(); mesh.faces.add(face); } } if (y<nY-1){ float
	 * vY=get(x,y+1,z); if (v<iso!=vY<iso){ Face face=new
	 * Face(ng[x][y+1][z],ng[x][y+1][z+1],ng[x+1][y+1][z+1],ng[x+1][y+1][z]); if
	 * (v<iso)face.turn(); mesh.faces.add(face); } } if (z<nZ-1){ float
	 * vZ=get(x,y,z+1); if (v<iso!=vZ<iso){ Face face=new
	 * Face(ng[x][y][z+1],ng[x][y+1][z+1],ng[x+1][y+1][z+1],ng[x+1][y][z+1]); if
	 * (v<iso)face.turn(); mesh.faces.add(face); } } } } } for (int
	 * i=0;i<mesh.faces.size();i++){ Face face=mesh.faces.get(i); for (int
	 * ii=0;ii<face.vertices.size();ii++){ Node
	 * node=(Node)face.vertices.get(ii); if (node.id<0){
	 * node.id=mesh.vertices.size(); mesh.vertices.add(node); } } } }
	 */
	public Mesh getQuadMeshInterpolated(float iso) {
		Mesh mesh = new Mesh();
		Node[][][] ng = new Node[nX + 1][nY + 1][nZ + 1];
		for (int x = 0; x < nX + 1; x++) {
			for (int y = 0; y < nY + 1; y++) {
				for (int z = 0; z < nZ + 1; z++) {
					// here the average of intersections
					ng[x][y][z] = new Node(x - 0.5f, y - 0.5f, z - 0.5f);
					ng[x][y][z].id = -1;
				}
			}
		}
		for (int x = 0; x < nX; x++) {
			for (int y = 0; y < nY; y++) {
				for (int z = 0; z < nZ; z++) {
					float v = get(x, y, z);
					if (x < nX - 1) {
						float vX = get(x + 1, y, z);
						if (v < iso != vX < iso) {
							Face face = new Face(ng[x + 1][y][z],
									ng[x + 1][y][z + 1],
									ng[x + 1][y + 1][z + 1],
									ng[x + 1][y + 1][z]);
							if (v < iso)
								face.turn();
							mesh.faces.add(face);
						}
					}
					if (y < nY - 1) {
						float vY = get(x, y + 1, z);
						if (v < iso != vY < iso) {
							Face face = new Face(ng[x][y + 1][z],
									ng[x][y + 1][z + 1],
									ng[x + 1][y + 1][z + 1],
									ng[x + 1][y + 1][z]);
							if (v < iso)
								face.turn();
							mesh.faces.add(face);
						}
					}
					if (z < nZ - 1) {
						float vZ = get(x, y, z + 1);
						if (v < iso != vZ < iso) {
							Face face = new Face(ng[x][y][z + 1],
									ng[x][y + 1][z + 1],
									ng[x + 1][y + 1][z + 1],
									ng[x + 1][y][z + 1]);
							if (v < iso)
								face.turn();
							mesh.faces.add(face);
						}
					}
				}
			}
		}
		for (int i = 0; i < mesh.faces.size(); i++) {
			Face face = mesh.faces.get(i);
			for (int ii = 0; ii < face.vertices.size(); ii++) {
				Node node = (Node) face.vertices.get(ii);
				if (node.id < 0) {
					node.id = mesh.vertices.size();
					mesh.vertices.add(node);
				}
			}
		}
		return mesh;
	}

	public Mesh getQuadMesh(float iso) {
		Mesh mesh = new Mesh();
		Node[][][] ng = new Node[nX + 1][nY + 1][nZ + 1];
		for (int x = 0; x < nX + 1; x++) {
			for (int y = 0; y < nY + 1; y++) {
				for (int z = 0; z < nZ + 1; z++) {
					ng[x][y][z] = new Node(x - 0.5f, y - 0.5f, z - 0.5f);
					ng[x][y][z].id = -1;
				}
			}
		}
		for (int x = 0; x < nX; x++) {
			for (int y = 0; y < nY; y++) {
				for (int z = 0; z < nZ; z++) {
					float v = get(x, y, z);
					if (x < nX - 1) {
						float vX = get(x + 1, y, z);
						if (v < iso != vX < iso) {
							Face face = new Face(ng[x + 1][y][z],
									ng[x + 1][y][z + 1],
									ng[x + 1][y + 1][z + 1],
									ng[x + 1][y + 1][z]);
							if (v < iso)
								face.turn();
							mesh.faces.add(face);
						}
					}
					if (y < nY - 1) {
						float vY = get(x, y + 1, z);
						if (v < iso != vY < iso) {
							Face face = new Face(ng[x][y + 1][z],
									ng[x][y + 1][z + 1],
									ng[x + 1][y + 1][z + 1],
									ng[x + 1][y + 1][z]);
							if (v < iso)
								face.turn();
							mesh.faces.add(face);
						}
					}
					if (z < nZ - 1) {
						float vZ = get(x, y, z + 1);
						if (v < iso != vZ < iso) {
							Face face = new Face(ng[x][y][z + 1],
									ng[x][y + 1][z + 1],
									ng[x + 1][y + 1][z + 1],
									ng[x + 1][y][z + 1]);
							if (v < iso)
								face.turn();
							mesh.faces.add(face);
						}
					}
				}
			}
		}
		for (int i = 0; i < mesh.faces.size(); i++) {
			Face face = mesh.faces.get(i);
			for (int ii = 0; ii < face.vertices.size(); ii++) {
				Node node = (Node) face.vertices.get(ii);
				if (node.id < 0) {
					node.id = mesh.vertices.size();
					mesh.vertices.add(node);
				}
			}
		}
		return mesh;
	}

	/**
	 * @param iso
	 * @return
	 */
	public int[] createFaces(float iso) {
		int nQuads = 0;

		for (int x = 0; x < nX - 1; x++) {
			for (int y = 0; y < nY - 1; y++) {
				for (int z = 0; z < nZ - 1; z++) {
					int index = getIndex(x, y, z);
					int indexX = index + nYZ;
					int indexY = index + nZ;
					int indexZ = index + 1;
					if (values[index] <= iso != values[indexX] <= iso)
						nQuads++;
					if (values[index] <= iso != values[indexY] <= iso)
						nQuads++;
					if (values[index] <= iso != values[indexZ] <= iso)
						nQuads++;
				}
			}
		}
		int[] quads = new int[nQuads * 4];
		nQuads = 0;
		int iQuad = 0;
		for (int x = 0; x < nX - 1; x++) {
			for (int y = 0; y < nY - 1; y++) {
				for (int z = 0; z < nZ - 1; z++) {
					int index = getIndex(x, y, z);
					int indexX = index + nYZ;
					int indexY = index + nZ;
					int indexZ = index + 1;
					if (values[index] <= iso != values[indexX] <= iso) {
						quads[iQuad] = index + nYZ;
						quads[iQuad + 1] = index + nZ + nYZ;
						quads[iQuad + 2] = index + nZ + 1 + nYZ;
						quads[iQuad + 3] = index + 1 + nYZ;
						iQuad += 4;
					}

					if (values[index] <= iso != values[indexY] <= iso) {
						quads[iQuad] = index + nZ;
						quads[iQuad + 1] = index + nYZ + nZ;
						quads[iQuad + 2] = index + nYZ + 1 + nZ;
						quads[iQuad + 3] = index + 1 + nZ;
						iQuad += 4;
					}

					if (values[index] <= iso != values[indexZ] <= iso) {
						quads[iQuad] = index + 1;
						quads[iQuad + 1] = index + nZ + 1;
						quads[iQuad + 2] = index + nZ + nYZ + 1;
						quads[iQuad + 3] = index + nYZ + 1;
						iQuad += 4;
					}
				}
			}
		}
		return quads;
	}

	/**
	 * @param g
	 * @param iso
	 */
	public void displayMCube(PGraphics g, float iso) {
		if (mCube == null) {
			createMCube(iso);
		}
		mCube.draw(g);

	}

	/**
	 * @param g
	 */
	public void display2D(PGraphics g) {
		for (int x = 0; x < nX; x++) {
			for (int y = 0; y < nY; y++) {
				g.set(x, y, g.color(get(x, y, 0)));
			}
		}
	}

	/**
	 * @param g
	 * @param iso
	 * @param dim
	 */
	public void displayGrid(PGraphics g, float iso, float dim) {
		for (int x = 0; x < nX; x++) {
			for (int y = 0; y < nY; y++) {
				for (int z = 0; z < nZ; z++) {
					if (isBorder(x, y, z, iso)) {
						g.pushMatrix();
						g.translate(x, y, z);
						if (Math.random() > 0.7f)
							;
						g.box(dim, dim, 1);
						g.box(dim, 1, dim);
						g.pushMatrix();
						g.translate(0, dim, 0);
						g.box(1, dim, dim);
						g.popMatrix();
						g.popMatrix();
					}
				}
			}
		}
	}

	public void displayBoxes(PGraphics g, float iso, float size) {
		for (int x = 0; x < nX; x++) {
			for (int y = 0; y < nY; y++) {
				for (int z = 0; z < nZ; z++) {
					if (isBorder(x, y, z, iso)) {
						g.pushMatrix();
						g.translate(x, y, z);
						g.box(size);
						g.popMatrix();
					}
				}
			}
		}
	}

	public void displayQuads(PGraphics g, float iso) {
		g.beginShape(PGraphics.QUADS);
		for (int x = 0; x < nX - 1; x++) {
			for (int y = 0; y < nY - 1; y++) {
				for (int z = 0; z < nZ - 1; z++) {
					int index = getIndex(x, y, z);
					int indexX = index + nYZ;
					int indexY = index + nZ;
					int indexZ = index + 1;
					if (values[index] <= iso != values[indexX] <= iso) {
						g.vertex(x + 1, y, z);
						g.vertex(x + 1, y, z + 1);
						g.vertex(x + 1, y + 1, z + 1);
						g.vertex(x + 1, y + 1, z);
					}
					if (values[index] <= iso != values[indexY] <= iso) {
						g.vertex(x, y + 1, z);
						g.vertex(x, y + 1, z + 1);
						g.vertex(x + 1, y + 1, z + 1);
						g.vertex(x + 1, y + 1, z);
					}
					if (values[index] <= iso != values[indexZ] <= iso) {
						g.vertex(x, y, z + 1);
						g.vertex(x, y + 1, z + 1);
						g.vertex(x + 1, y + 1, z + 1);
						g.vertex(x + 1, y, z + 1);
					}
				}
			}
		}
		g.endShape();

	}

	public void exportBoxesObj(String pathName, float iso, float s) {
		File file = new File(pathName);
		FileWriter fw;
		int id = 1;
		try {
			fw = new FileWriter(file);
			BufferedWriter bw = new BufferedWriter(fw);

			for (int x = 0; x < nX; x++) {
				for (int y = 0; y < nY; y++) {
					for (int z = 0; z < nZ; z++) {
						if (isBorder(x, y, z, iso)) {
							id = writeBox(id, x, y, z, s, bw);
						}
					}
				}
			}
			bw.flush();
			bw.close();
			System.out.println("OBJ exported: " + file);

		} catch (IOException e1) {
			// TODO Auto-generated catch block
			e1.printStackTrace();
		}
	}

	public int writeBox(int id, int x, int y, int z, float s, BufferedWriter w) {
		try {
			w.write("v " + x + " " + y + " " + z);
			w.newLine();
			w.write("v " + x + " " + (y + s) + " " + z);
			w.newLine();
			w.write("v " + (x + s) + " " + (y + s) + " " + z);
			w.newLine();
			w.write("v " + (x + s) + " " + y + " " + z);
			w.newLine();
			w.write("v " + x + " " + y + " " + (z + s));
			w.newLine();
			w.write("v " + x + " " + (y + s) + " " + (z + s));
			w.newLine();
			w.write("v " + (x + s) + " " + (y + s) + " " + (z + s));
			w.newLine();
			w.write("v " + (x + s) + " " + y + " " + (z + s));
			w.newLine();
			w.write("f " + id + " " + (id + 1) + " " + (id + 2) + " "
					+ (id + 3));
			w.newLine();
			w.write("f " + (id + 0) + " " + (id + 1) + " " + (id + 5) + " "
					+ (id + 4));
			w.newLine();
			w.write("f " + (id + 1) + " " + (id + 2) + " " + (id + 6) + " "
					+ (id + 5));
			w.newLine();
			w.write("f " + (id + 2) + " " + (id + 3) + " " + (id + 7) + " "
					+ (id + 6));
			w.newLine();
			w.write("f " + (id + 3) + " " + (id + 0) + " " + (id + 4) + " "
					+ (id + 7));
			w.newLine();
			w.write("f " + (id + 4) + " " + (id + 5) + " " + (id + 6) + " "
					+ (id + 7));
			w.newLine();
			return id + 8;
		} catch (IOException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
			return id;
		}
	}

	public int writeQuad(int vId, int[] quadIds, int[] vertices,
			BufferedWriter w) {
		try {
			for (int i = 0; i < quadIds.length; i++) {
				int qi = quadIds[i];
				if (vertices[qi] < 0) {
					vertices[qi] = vId;
					vId++;
					float cX = getXFloat(getX(qi));
					float cY = getYFloat(getY(qi));
					float cZ = getZFloat(getZ(qi));
					w.write("v " + cX + " " + cY + " " + cZ);
					w.newLine();
				}
			}
			String quad = "f";
			for (int i = 0; i < quadIds.length; i++) {
				int qi = quadIds[i];
				quad += " " + vertices[qi];

			}
			w.write(quad);
			w.newLine();

		} catch (IOException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
		return vId;
	}

	public void saveObj(float iso, String pathName) {
		File file = new File(pathName);
		FileWriter fw;
		try {
			fw = new FileWriter(file);
			BufferedWriter bw = new BufferedWriter(fw);
			int[] vertices = new int[values.length];
			Arrays.fill(vertices, -1);
			int iV = 1;
			int cI = 0;
			int[] quadIds = new int[4];
			for (int x = 0; x < nX - 1; x++) {
				for (int y = 0; y < nY - 1; y++) {
					for (int z = 0; z < nZ - 1; z++) {
						int index = getIndex(x, y, z);
						int indexX = index + nYZ;
						int indexY = index + nZ;
						int indexZ = index + 1;
						if (values[index] <= iso != values[indexX] <= iso) {
							quadIds[0] = index + nYZ;
							quadIds[1] = index + nZ + nYZ;
							quadIds[2] = index + nZ + 1 + nYZ;
							quadIds[3] = index + 1 + nYZ;
							iV = writeQuad(iV, quadIds, vertices, bw);
						}

						if (values[index] <= iso != values[indexY] <= iso) {
							quadIds[0] = index + nZ;
							quadIds[1] = index + nYZ + nZ;
							quadIds[2] = index + nYZ + 1 + nZ;
							quadIds[3] = index + 1 + nZ;
							iV = writeQuad(iV, quadIds, vertices, bw);
						}

						if (values[index] <= iso != values[indexZ] <= iso) {
							quadIds[0] = index + 1;
							quadIds[1] = index + nZ + 1;
							quadIds[2] = index + nZ + nYZ + 1;
							quadIds[3] = index + nYZ + 1;
							iV = writeQuad(iV, quadIds, vertices, bw);
						}
					}
				}
			}

			bw.flush();
			bw.close();
			System.out.println("OBJ exported: " + file);
		} catch (IOException e1) {
			// TODO Auto-generated catch block
			e1.printStackTrace();
		}
	}

	public void calculateDepth() {
		Grid3DDepth.calculateDepth(values, nX, nY, nZ, d1, d2, d3);
	}

}